#include <gtest/gtest.h>

#include "dart/lcpsolver/Lemke.h"
#include "TestHelpers.h"

 
//==============================================================================
TEST(Lemke, Lemke_1D)
{
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Eigen::VectorXd* f;
    int err;

    f =  new Eigen::VectorXd(1);
    A.resize(1,1);
    A << 1;
    b.resize(1);
    b << -1.5;
    err = dart::lcpsolver::Lemke(A,b,f);

    EXPECT_EQ(err, 0);
    EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_2D)
{
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Eigen::VectorXd* f;
    int err;

    f =  new Eigen::VectorXd(2);
    A.resize(2,2);
    A << 3.12, 0.1877, 0.1877, 3.254;
    b.resize(2);
    b << -0.00662, -0.006711;
    err = dart::lcpsolver::Lemke(A,b,f);

    EXPECT_EQ(err, 0);
    EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_4D)
{
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Eigen::VectorXd* f;
    int err;

    f =  new Eigen::VectorXd(4);
    A.resize(4,4);
    A <<
             3.999,0.9985, 1.001,    -2,
            0.9985, 3.998,    -2,0.9995,
             1.001,    -2, 4.002, 1.001,
                -2,0.9995, 1.001, 4.001;

    b.resize(4);
    b << 
             -0.01008,
            -0.009494,
             -0.07234,
             -0.07177;

    err = dart::lcpsolver::Lemke(A,b,f);

    EXPECT_EQ(err, 0);
    EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_6D)
{
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Eigen::VectorXd* f;
    int err;

    f =  new Eigen::VectorXd(6);
    A.resize(6,6);
    A <<
            3.1360,   -2.0370,   0.9723,   0.1096,  -2.0370,   0.9723,
           -2.0370,    3.7820,   0.8302,  -0.0257,   2.4730,   0.0105,
            0.9723,    0.8302,   5.1250,  -2.2390,  -1.9120,   3.4080,
            0.1096,   -0.0257,  -2.2390,   3.1010,  -0.0257,  -2.2390,
           -2.0370,    2.4730,  -1.9120,  -0.0257,   5.4870,  -0.0242,
            0.9723,    0.0105,   3.4080,  -2.2390,  -0.0242,   3.3860;

    b.resize(6);
    b <<
            0.1649,
           -0.0025,
           -0.0904,
           -0.0093,
           -0.0000,
           -0.0889;

    err = dart::lcpsolver::Lemke(A,b,f);

    EXPECT_EQ(err, 0);
    EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_12D)
{
    Eigen::MatrixXd A;
    Eigen::VectorXd b;
    Eigen::VectorXd* f;
    int err;

    f =  new Eigen::VectorXd(12);
    A.resize(12,12);
    A <<
               4.03, -1.014, -1.898,   1.03, -1.014, -1.898,      1, -1.014, -1.898,     -2, -1.014, -1.898,
             -1.014,  4.885, -1.259,  1.888,   3.81,  2.345, -1.879,  1.281, -2.334,  1.022,  0.206,   1.27,
             -1.898, -1.259,    3.2, -1.032,-0.6849,  1.275,  1.003, 0.6657,  3.774,  1.869,   1.24,   1.85,
               1.03,  1.888, -1.032,   4.03,  1.888, -1.032,     -2,  1.888, -1.032,      1,  1.888, -1.032,
             -1.014,   3.81,-0.6849,  1.888,  3.225,  1.275, -1.879,   1.85,  -1.27,  1.022,  1.265, 0.6907,
             -1.898,  2.345,  1.275, -1.032,  1.275,   4.86,  1.003,  -1.24, 0.2059,  1.869, -2.309,  3.791,
                  1, -1.879,  1.003,     -2, -1.879,  1.003,   3.97, -1.879,  1.003, 0.9703, -1.879,  1.003,
             -1.014,  1.281, 0.6657,  1.888,   1.85,  -1.24, -1.879,  3.187,  1.234,  1.022,  3.755,-0.6714,
             -1.898, -2.334,  3.774, -1.032,  -1.27, 0.2059,  1.003,  1.234,  4.839,  1.869,  2.299,   1.27,
                 -2,  1.022,  1.869,      1,  1.022,  1.869, 0.9703,  1.022,  1.869,   3.97,  1.022,  1.869,
             -1.014,  0.206,   1.24,  1.888,  1.265, -2.309, -1.879,  3.755,  2.299,  1.022,  4.814,  -1.25,
             -1.898,   1.27,   1.85, -1.032, 0.6907,  3.791,  1.003,-0.6714,   1.27,  1.869,  -1.25,  3.212;

    b.resize(12);
    b <<
              -0.00981,
            -1.458e-10,
             5.357e-10,
               -0.0098,
             -1.44e-10,
             5.298e-10,
             -0.009807,
            -1.399e-10,
             5.375e-10,
             -0.009807,
            -1.381e-10,
             5.316e-10;

    err = dart::lcpsolver::Lemke(A,b,f);

    EXPECT_EQ(err, 0);
    EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
int main(int argc, char* argv[])
{
    ::testing::InitGoogleTest(&argc,argv);
    return RUN_ALL_TESTS();
}
